# 在这里写上你的代码 :-)
# -*- coding:utf-8 -*-
# 导入相关模块
from machine import Pin,PWM,ADC
from NeoPID import PID
import time                
class PWMCarControl:
    def __init__(self):
        #LeftPin
        self.L1 = PWM(Pin(39), freq=1000, duty=0) # 初始化PWM 。这个PWM由32号引脚输出，频率为1000，初始占空比为0）。
        self.L2 = PWM(Pin(38), freq=1000, duty=0) # 初始化PWM 。这个PWM由33号引脚输出，频率为1000，初始占空比为0）。
        #RightPin
        self.R1 = PWM(Pin(37), freq=1000, duty=0)
        self.R2 = PWM(Pin(36), freq=1000, duty=0)
        #duoji
        self.p5 = PWM(Pin(5) , freq=50  , duty=0)
        #
        self.p1 = Pin(1, Pin.IN)
        self.p4 = Pin(4, Pin.IN)
        self.speed = 980 # 定义初始速度
        self.speed2 = 980

    def CarRun(self,adj):
        s1=self.p1.value()
        s2=self.p4.value()
        if adj<0 :
            self.L1.duty(self.speed2)
            self.L2.duty(0)
            self.R1.duty(self.speed + adj)
            self.R2.duty(0)
            
        elif adj>0 :
            self.L1.duty(self.speed - adj)
            self.L2.duty(0)
            self.R1.duty(self.speed)
            self.R2.duty(0)
        
        else:
            self.L1.duty(self.speed2)
            self.L2.duty(0)
            self.R1.duty(self.speed)
            self.R2.duty(0)
    def move(self,s):
        if s==1:
            self.L1.duty(770)
            self.L2.duty(0)
            self.R1.duty(820)
            self.R2.duty(0)
    def duoji(self,r):
        if r==1:
            self.p5.duty(2)
class ADC_sensor:
    def __init__(self):
        self.in1 = ADC(Pin(2))
        self.in2 = ADC(Pin(3))
        self.in3 = ADC(Pin(5))
        self.in4 = ADC(Pin(4))
        self.in1.atten(ADC.ATTN_11DB)  # 这里配置测量量程为3.3V
        self.in2.atten(ADC.ATTN_11DB)
        self.in3.atten(ADC.ATTN_11DB)
        self.in4.atten(ADC.ATTN_11DB)
        self.wf1=Wave_filtering(500)
        self.wf2=Wave_filtering(500)
        self.wf3=Wave_filtering(500)
        self.wf4=Wave_filtering(500)
           
    def read_value(self):
        v1=self.in1.read()+0 # 消除零点偏差
        v2=self.in2.read()+910
        if v2>8191:
            v2 = 8191
        v3=self.in3.read()+0
        v4=self.in4.read()+0
        v1=int(self.wf1.filtering(v1))
        v2=int(self.wf2.filtering(v2))
        v3=int(self.wf3.filtering(v3))
        v4=int(self.wf4.filtering(v4))
        return (v1,v2,v3,v4)
    
class Wave_filtering():
    def __init__(self,k):
        self.a=[6000,6000,6000]
        self.c=k
    def filtering(self,d):
        s=0
        for i in self.a:
            s = s + i
        g=int(s/3)
        if d<g-self.c:
            o = self.a[0]-self.c
        elif d>g+self.c:
            o = self.a[0]+self.c
        else:
            o = d
        self.a[2]=self.a[1]
        self.a[1]=self.a[0]
        self.a[0]=o
        return o
if __name__ == '__main__':
    WMCar = PWMCarControl()
    sen=ADC_sensor()
    pid=PID(0.65,0.3,0.1,input_lim=8191,output_lim=1000)
    while True:
        
        adc_value=sen.read_value()
        err=adc_value[0]-adc_value[1]
        a=(adc_value[0]+adc_value[1])/2
        adj=3*int(pid.get_adj_value(err))
        if adj>800:
            adj=800
        elif adj<-800:
            adj=-800
        if a<5000:
            WMCar.move(1)
        else:
            WMCar.CarRun(adj)
        #print(adj,err,adc_value,a)
